#pragma once

#include <cmath>
#include <Eigen/Core>

enum class CoordinateAxis
{
    X,
    Y,
    Z
};

template <typename T>
Eigen::Matrix<T, 3, 3> coordinateRotation(CoordinateAxis axis, T theta)
{
    static_assert(std::is_floating_point<T>::value,
                  "must use floating point value");
    T s = std::sin(theta);
    T c = std::cos(theta);

    Eigen::Matrix<T, 3, 3> R;

    if (axis == CoordinateAxis::X)
    {
        R << 1, 0, 0, 0, c, s, 0, -s, c;
    }
    else if (axis == CoordinateAxis::Y)
    {
        R << c, 0, -s, 0, 1, 0, s, 0, c;
    }
    else if (axis == CoordinateAxis::Z)
    {
        R << c, s, 0, -s, c, 0, 0, 0, 1;
    }

    return R;
}

template <typename T>
Eigen::Matrix<T, 3, 3> rpyToRotMat(Eigen::Matrix<T, 3, 1> &v)
{
    Eigen::Matrix<T, 3, 3> m = coordinateRotation(CoordinateAxis::X, v[0]) *
                               coordinateRotation(CoordinateAxis::Y, v[1]) *
                               coordinateRotation(CoordinateAxis::Z, v[2]);
    return m;
}

template <typename T>
Eigen::Matrix<T, 3, 1> quatToRPY(const Eigen::Matrix<T, 4, 1> &q)
{
    Eigen::Matrix<T, 3, 1> rpy;
    T as = std::min(-2. * (q[1] * q[3] - q[0] * q[2]), .99999);
    rpy(2) =
        std::atan2(2 * (q[1] * q[2] + q[0] * q[3]),
                   square(q[0]) + square(q[1]) - square(q[2]) - square(q[3]));
    rpy(1) = std::asin(as);
    rpy(0) =
        std::atan2(2 * (q[2] * q[3] + q[0] * q[1]),
                   square(q[0]) - square(q[1]) - square(q[2]) + square(q[3]));
    return rpy;
}

template <typename T>
Eigen::Matrix<T, 3, 3> quatToRotMat(const Eigen::Matrix<T, 4, 1> &q)
{
    T e0 = q(0);
    T e1 = q(1);
    T e2 = q(2);
    T e3 = q(3);

    Eigen::Matrix<T, 3, 3> R;

    R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
        2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
        1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
        2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
        1 - 2 * (e1 * e1 + e2 * e2);
    R.transposeInPlace();
    return R;
}

template <typename T>
Eigen::Matrix<T, 3, 3> skew(Eigen::Matrix<T, 3, 1> &vec)
{
    Eigen::Matrix<T, 3, 3> rst;
    rst.setZero();
    rst << 0, -vec(2), vec(1),
        vec(2), 0, -vec(0),
        -vec(1), vec(0), 0;
    return rst;
}
